#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include "mpu6050.h"
#include "mpu6050glob.h"
#include "armsdkerr.h"

INT4 armDrvMpu6050Enable()
{
    if (gmpuState == TRUE)
    {
        PRINTF("mpu6050 is already opened\n");
        return FAILURE;
    }

    PRINTF("enable mpu6050...\n");
    mpuFilefd = open(pu1MpuDrvFile, O_RDWR);
    if(mpuFilefd < 0)
    {
        PRINTF("can't open file %s\r\n", pu1MpuDrvFile);
        return FAILURE;
    }

    gmpuState = TRUE;

    return SUCCESS;
}

BOOL armDrvMpu6050State()
{
    return gmpuState;
}

INT4 armDrvMpu6050Disable()
{
    PRINTF("mpu6050 module disable ...\n");
    close(mpuFilefd); /* 关闭文件 */

    gmpuState = FALSE;
    return SUCCESS;
}

INT4 armDrvMpu6050Output(INT4 i4num)
{
    INT4 ret = 0;
    INT4 i4Inc = 0;
    mpu6050_data_t mpuData;
    MEMSET(&mpuData, 0, sizeof(mpu6050_data_t));

    printf("time\tx_acc(m/s²)\ty_acc(m/s²)\tz_acc(m/s²)\tyaw\tpitch\troll\ttemerature\n");
    printf("--------------------------------------------------------------------------------\n");
    for (i4Inc = 0; i4Inc < i4num; i4Inc ++)
    {
        ret = armDrvMpu6050Get(&mpuData);
        if(ret == SUCCESS)  /* 数据读取成功 */
        {
            printf("%dms\t|", i4Inc * 5);
            printf("%.2f\t\t|%.2f\t\t|%.2f\t\t|",mpuData.acc_x, mpuData.acc_y, mpuData.acc_z);
            printf("%.2f°\t|%.2f°\t|%.2f°\t|",mpuData.yaw, mpuData.pitch, mpuData.roll);
            printf("%.2f°C\n", mpuData.temperature);
#if VOFA
            printf("samples:%f,%f,%f,%f\n",mpuData.yaw, mpuData.pitch, mpuData.roll, mpuData.temperature);
#endif
            printf("--------------------------------------------------------------------------------\n");
        }
        else
        {
            return FAILURE;
        }
    }

    return SUCCESS;
}

INT4 armDrvMpu6050Get(mpu6050_data_t *mpudata)
{
    short databuf[8];
    INT4 ret = 0;

    double yaw_temp = 0;
    double pitch_temp = 0;
    double roll_temp = 0;
    
    ret = read(mpuFilefd, databuf, sizeof(databuf));
    if(ret == 0)  /* 数据读取成功 */
    {
        mpudata->acc_x   = (double)databuf[0] * 5.9814e-4f; // data_raw * 2 *  9.8 / 32768
        mpudata->acc_y   = (double)databuf[1] * 5.9814e-4f;
        mpudata->acc_z   = (double)databuf[2] * 5.9814e-4f;
        mpudata->angle_x = (double)databuf[3] * 6.1035e-2f; //data_raw *  2000 /32768
        mpudata->angle_y = (double)databuf[4] * 6.1035e-2f;
        mpudata->angle_z = (double)databuf[5] * 6.1035e-2f;
        mpudata->temperature = (double)databuf[6] / 340 + 36.53;  // 数据以mpu6050芯片为准
        //mpudata->temperature = databuf[6] / 333.87 + 21; // 数据以mpu6500芯片为准, 灵敏度系数：333.87

        yaw_temp = mpudata->yaw + mpudata->angle_z * 0.005;  // yaw = yaw + gz * Δt, Δt这里采样的频率是5ms
        pitch_temp = mpudata->pitch + mpudata->angle_x * 0.005;
        roll_temp = mpudata->roll + mpudata->angle_y * 0.005;

        mpudata->yaw = (int)yaw_temp % 360 + (yaw_temp - (int)yaw_temp);
        mpudata->pitch = (int)pitch_temp % 360 + (pitch_temp - (int)pitch_temp);
        mpudata->roll = (int)roll_temp % 360 + (roll_temp - (int)roll_temp);

        usleep(5000);
        memset(databuf, 0, sizeof(databuf));

        return SUCCESS;
    }
    
    return FAILURE;
}
